function [F]=fundamental
addpath SiftDist/
addpath SiftDist/sift/

%Ofir's code
%%%%%%%%%%%%%%%%%%%
I1=imreadbw('left.png');
I2=imreadbw('right.png');

DistRatio= 1.25;
NumOrientBins= 16;
I1=I1-min(I1(:));
I1=I1/max(I1(:));
I2=I2-min(I2(:));
I2=I2/max(I2(:));

fprintf('Computing frames and descriptors (~0.5 minutes).\n');
[frames1,descr1]= sift(I1, 'NumOrientBins', NumOrientBins);
[frames2,descr2]= sift(I2, 'NumOrientBins', NumOrientBins);

fprintf('Matches using SiftDist (~1 minutes with default params).\n');
% Taking the sqrt of descr improves results.
% I still have no theoretical reason for why it improves results.
[inds ratios] = SiftRatioMatch(sqrt(descr1), frames1, sqrt(descr2), frames2, DistRatio);


% Transform to Andrea's Vedaldi's matches format
matches= zeros(2,sum(inds~=0));
i= 1;
for r=1:size(descr1,2)
  if (inds(r)~=0)
    matches(1,i)= r;
    matches(2,i)= inds(r);
    i=i+1;
  end
end

% 
% figure; clf;
% title('Click on a keypoint to see its match'); axis off;
% plotmatches(I1, I2, frames1(1:2,:), frames2(1:2,:), matches, 'Interactive', 2);
% drawnow;
global x1;
global x2;
x1=frames1(1:2,matches(1,:));
x1(3,:)=1;
x2=frames2(1:2,matches(2,:));
x2(3,:)=1;
t=0.05;p=0.98;
best_F=NaN;
best_num_inliers=0;
best_inliers=[];
maxTrials = 100000;
count=0;
N=1;
good=1;
while N>count
    pts=randsample(size(matches,2),7);
    temp_x1=x1(:,pts(:));
    temp_x2=x2(:,pts(:));
    current_F=CalculateFundamentalMatrixOutOf7(temp_x1,temp_x2);
    [current_F current_inliers]=F_dist(current_F,x1,x2,t);
    n_inliers=length(current_inliers);
    if n_inliers>best_num_inliers
       % sprintf('here\n')
       good=good+1
        best_num_inliers=n_inliers
        best_inliers=current_inliers;
        best_F=current_F;
        %update N:
        fracinliers =  n_inliers/length(matches);
        pNoInliers = 1 -  fracinliers^7;
        pNoInliers = max(10^-10, pNoInliers);  % Avoid division by -Inf
        pNoInliers = min(1-10^-10, pNoInliers);% Avoid division by 0.
        N = log(1-p)/log(pNoInliers)
    end    
    count=count+1;
    if count>maxTrials
        sprintf('reached maximum trials');
        break;
    end
end
if best_F~=NaN
    F=best_F;
else
    F=0;
end
inliers1=x1(1:2,best_inliers);inliers2=x2(1:2,best_inliers)
figure; clf;
title('Click on a keypoint to see its match'); axis off;
plotmatches(I1, I2, inliers1(1:2,:), inliers2(1:2,:), [1:best_num_inliers;1:best_num_inliers], 'Interactive', 2);
drawnow;
%-----------------------------------------------------------
%given a vector with length 8 of sample points, compute F
function [F]=findF(cur_x1,cur_x2)
% match_ind=matches(:,pts);
% x1=frames1(1:2,match_ind(1,:));
% x1(3,:)=1;
% x2=frames2(1:2,match_ind(2,:));
% x2(3,:)=1;
% global x1;global x2;%normalized
% cur_x1=x1(:,pts(:));
% cur_x2=x2(:,pts(:));
%normalize
% std_cur_x1=std(cur_x1');
% center_cur_x1=sum(cur_x1')./8;
% center_cur_x1=center_cur_x1./std_cur_x1;
% T1=[1/std_cur_x1(1) 0 -center_cur_x1(1);
%     0 1/std_cur_x1(2) -center_cur_x1(2);
%     0 0 1];
% cur_x1=T1*cur_x1;
% %------
% std_cur_x2=std(cur_x2');
% center_cur_x2=sum(cur_x2')./8;
% center_cur_x2=center_cur_x2./std_cur_x2;
% T2=[1/std_cur_x2(1) 0 -center_cur_x2(1);
%     0 1/std_cur_x2(2) -center_cur_x2(2);
%     0 0 1];
% cur_x2=T2*cur_x2;
A = [cur_x2(1,:)'.*cur_x1(1,:)'  cur_x2(1,:)'.*cur_x1(2,:)'  cur_x2(1,:)' ...
         cur_x2(2,:)'.*cur_x1(1,:)'   cur_x2(2,:)'.*cur_x1(2,:)'  cur_x2(2,:)' ...
         cur_x1(1,:)'             cur_x1(2,:)'            ones(8,1) ];  
[u s v]=svd(A);
F = reshape(v(:,9),3,3)';
[u s v]=svd(F);
F=u*diag([s(1,1) s(2,2) 0])*v';
% Denormalise
%F = T2'*F*T1;


%---------------------------------------------------------
function [best_F bestInliers] = F_dist(F, x1,x2, t)
    bestInliers=NaN;
    nF = length(F);   % Number of solutions to test
	best_F = F{1};     % Initial allocation of best solution
	ninliers = 0;     % Number of inliers
	
	for k = 1:nF
	    x2tFx1 = zeros(1,length(x1));
	    for n = 1:length(x1)
		x2tFx1(n) = x2(:,n)'*F{k}*x1(:,n);
	    end
	    
	    Fx1 = F{k}*x1;
	    Ftx2 = F{k}'*x2;     

	    % Evaluate distances
	    d =  x2tFx1.^2 ./ ...
		 (Fx1(1,:).^2 + Fx1(2,:).^2 + Ftx2(1,:).^2 + Ftx2(2,:).^2);
	    
	    inliers = find(abs(d) < t);     % Indices of inlying points
	    
	    if length(inliers) > ninliers   % Record best solution
            ninliers = length(inliers);
            best_F = F{k};
            bestInliers = inliers;
	    end
	end

% else
%     x2tFx1 = zeros(1,length(x1));
% 	for n = 1:length(x1)
% 	    x2tFx1(n) = x2(:,n)'*F*x1(:,n);
%     end
% 	
% 	Fx1 = F*x1;
% 	Ftx2 = F'*x2;     
% 	
% 	% Evaluate distances
% 	d =  x2tFx1.^2 ./ ...
% 	     (Fx1(1,:).^2 + Fx1(2,:).^2 + Ftx2(1,:).^2 + Ftx2(2,:).^2);
% 	
% 	bestInliers = find(abs(d) < t);     % Indices of inlying points    